﻿#include "parabola_fitting_vertex.h"

void solveLinearSystem(float A[3][3], float b[3], float x[3]) {
    int i, j, k;
    float temp, factor;

    // 高斯消元
    for (i = 0; i < 3; i++) {
        // 寻找主元
        int maxRow = i;
        for (j = i + 1; j < 3; j++) {
            if (fabs(A[j][i]) > fabs(A[maxRow][i])) {
                maxRow = j;
            }
        }

        // 交换行
        if (maxRow != i) {
            for (j = i; j < 3; j++) {
                temp = A[i][j];
                A[i][j] = A[maxRow][j];
                A[maxRow][j] = temp;
            }
            temp = b[i];
            b[i] = b[maxRow];
            b[maxRow] = temp;
        }

        // 归一化主行
        factor = A[i][i];
        for (j = i; j < 3; j++) {
            A[i][j] /= factor;
        }
        b[i] /= factor;

        // 消去其他行
        for (j = 0; j < 3; j++) {
            if (j != i && A[j][i] != 0) {
                factor = A[j][i];
                for (k = i; k < 3; k++) {
                    A[j][k] -= factor * A[i][k];
                }
                b[j] -= factor * b[i];
            }
        }
    }

    // 结果
    for (i = 0; i < 3; i++) {
        x[i] = b[i];
    }
}

void findParabolaVertex(int* x, int* y, int n, float* vertexX, float* vertexY) {
    // 至少需要3个点才能拟合抛物线
    if (n < 3 || x == NULL || y == NULL || vertexX == NULL || vertexY == NULL) {
        *vertexX = NAN;
        *vertexY = NAN;
        return;
    }

    // 计算最小二乘法所需的总和
    float sumX = 0, sumX2 = 0, sumX3 = 0, sumX4 = 0;
    float sumY = 0, sumXY = 0, sumX2Y = 0;

    for (int i = 0; i < n; i++) {
        float xi = (float)x[i];
        float yi = (float)y[i];
        float xi2 = xi * xi;
        float xi3 = xi2 * xi;
        float xi4 = xi3 * xi;

        sumX += xi;
        sumX2 += xi2;
        sumX3 += xi3;
        sumX4 += xi4;
        sumY += yi;
        sumXY += xi * yi;
        sumX2Y += xi2 * yi;
    }

    // 构建线性方程组 Ax = b，其中x = [a, b, c]^T
    float A[3][3] = {
        {sumX4, sumX3, sumX2},
        {sumX3, sumX2, sumX},
        {sumX2, sumX,   n}
    };

    float b[3] = { sumX2Y, sumXY, sumY };
    float coeff[3]; // 存储a, b, c

    // 求解方程组得到系数
    solveLinearSystem(A, b, coeff);
    float a = coeff[0];
    float b_val = coeff[1];
    float c = coeff[2];

    // 计算顶点坐标
    if (fabs(a) < 1e-10) {
        // a接近0，不是抛物线
        *vertexX = NAN;
        *vertexY = NAN;
    }
    else {
        *vertexX = -b_val / (2 * a);
        *vertexY = a * (*vertexX) * (*vertexX) + b_val * (*vertexX) + c;
    }
}
/**
 * 生成带有噪声的抛物线采样点
 *
 * @param x 输出参数，存储生成的x坐标数组
 * @param y 输出参数，存储生成的y坐标数组（带有噪声）
 * @param numPoints 要生成的采样点数量
 * @param vertexX 抛物线顶点的x坐标
 * @param vertexY 抛物线顶点的y坐标
 * @param a 抛物线开口大小参数（a>0开口向上，a<0开口向下，绝对值越大开口越窄）
 * @param noiseLevel 噪声水平（0表示无噪声，值越大噪声越大）
 * @param xStart x坐标起始值
 * @param xEnd x坐标结束值
 */
void generateNoisyParabola(int* x, int* y, int numPoints,
    float vertexX, float vertexY, float a,
    float noiseLevel, float xStart, float xEnd) {
    if (numPoints <= 0 || x == NULL || y == NULL || xStart >= xEnd) {
        printf("无效的参数\n");
        return;
    }

    // 初始化随机数生成器
    srand(time(NULL));

    // 计算x步长
    float step = (xEnd - xStart) / (numPoints - 1);

    // 生成每个采样点
    for (int i = 0; i < numPoints; i++) {
        // 生成x坐标
        x[i] = (int)(xStart + i * step);

        // 计算理想抛物线的y值 (顶点式: y = a(x - h)² + k)
        float dx = (float)x[i] - vertexX;
        float idealY = a * dx * dx + vertexY;

        // 生成噪声 (-noiseLevel 到 +noiseLevel 之间的随机值)
        float noise = ((float)rand() / RAND_MAX) * 2 * noiseLevel - noiseLevel;

        // 带噪声的y值
        y[i] = (int)(idealY + noise);
    }
}

//int main()
//{
//    int numPoints = 20;          // 采样点数量
//    float x[20];         // x坐标数组
//    float y[20];         // y坐标数组
//
//    // 抛物线参数
//    float vertexX = 1300.0;        // 顶点x坐标
//    float vertexY = 50.0;        // 顶点y坐标
//    float a = 1.0;              // 开口参数（正值表示开口向上，顶点为最小值）
//    float noiseLevel = 0.9;     // 噪声水平
//    float xStart = 1000.0;         // x起始值
//    float xEnd = 1666.0;           // x结束值
//
//    // 生成带噪声的抛物线采样点
//    generateNoisyParabola(x, y, numPoints, vertexX, vertexY, a, noiseLevel, xStart, xEnd);
//
//    // 打印结果
//    printf("生成的带噪声抛物线采样点：\n");
//    printf("x\t\t y\n");
//    printf("------------------------\n");
//    for (int i = 0; i < numPoints; i++) {
//        printf("%.4f\t %.4f\n", x[i], y[i]);
//    }
//
//
//    //float x[] = { 0, 1, 2, 3, 4, 5 };
//    //float y[] = { 5, 2, 1, 2, 5, 10 };
//    int n = sizeof(x) / sizeof(x[0]);
//
//    float vertexX1, vertexY1;
//    findParabolaVertex(x, y, n, &vertexX1, &vertexY1);
//
//    printf("抛物线顶点坐标: (%.4f, %.4f)\n", vertexX1, vertexY1);
//}
//

